#ifndef INSDRIVER_H
#define INSDRIVER_H

#include "bufferparaseinterface.h"
#include <iostream>
#include <vector>
#include <math.h>
#include <typeinfo>
#include "INS570D/INS570D_Define.h"
#include "INS_STATUS.h"

class INS570D_Driver : public BufferParaseInterface
{
public:
    INS570D_Driver(size_t size_);

    ~INS570D_Driver();
    uint8_t checksum();
    bool setBuffer(const std::vector<uint8_t>&buf) override;
    void parse() override;
    INS_STATUS dummyBuf2value();
    INS_STATUS& getInsData();
    RTK_STATUS& getRTKData(){}
    INS_STATUS& getIMUData(){}


    // 从字节流中截取片段（代表某个物理量的值）
    template<class T>
    T getOneValue(size_t offset_, size_t length_)
    {
        T buf_=T(0);
        for(int i=offset_,index=0;index<length_;i++,index++){
            buf_ += buffer_.at(i)<<(index*8);
            //printf("%x\n",buffer_.at(i));
        }
        return buf_;
    }

    // 16进制转十进制浮点数(有符号)
    template<class T>
    double hex2Value(T buf,double gain){
        double phyval=0;
        if(typeid(T) == typeid(uint16_t)){
            int16_t buf_i16=0;
            buf_i16 = *((int16_t*)(&buf));
            phyval = (double)buf_i16*gain;
            //printf("TYPE: [uint16_t] [%f] \n",(double)phyval);
        }else if(typeid(T) == typeid(uint32_t)){
            int32_t buf_i32=0;
            buf_i32 = *((int32_t*)(&buf));
            phyval = (double)buf_i32*gain;
        }
        return phyval;
    }

protected:
    size_t buffer_size_;
    bool buffer_updated;
    //unsigned char* buffer_;
    std::vector<uint8_t> buffer_;
    std::vector<uint32_t> dummy_array_;
    INS_STATUS ins570d;

    void getRPY(INS_STATUS& tmp_);
    void getGYRO(INS_STATUS& tmp_);
    void getACC(INS_STATUS& tmp_);
    void getWGS84(INS_STATUS& tmp_);
    void getVEL(INS_STATUS& tmp_);
    void getALISTATUS(INS_STATUS& tmp_);
    void getLOOPDATA(INS_STATUS& tmp_);
    void getDATA123(double data123[]);
    void getPOSI_STD(INS_STATUS& tmp_);
    void getVEL_STD(INS_STATUS& tmp_);
    void getPOSE_STD(INS_STATUS& tmp_);
    void getTEMPERTURE(INS_STATUS& tmp_);
    void getGPSSTATUS(INS_STATUS& tmp_);
    void getWHEELSPEEDSTATUS(INS_STATUS& tmp_);

};

#endif // INSDRIVER_H
